import sensor, image, time,lcd
from pyb import UART,Pin,Timer,Servo
from button import BUTTON
from umotor import UMOTOR

class PYCOMMON():
    def __init__(self):
        self.s_last=0
        self.button=BUTTON()
        self.motor=UMOTOR()
    def Auto_exposeure_sensor(self):
        img = sensor.snapshot()
        statistics=img.get_statistics()
        lmean=statistics.l_mean()
        EXPOSURE_TIME_SCALE=128/lmean
        print("expose scale:%f"%EXPOSURE_TIME_SCALE)
        sensor.set_auto_gain(True)
        sensor.set_auto_whitebal(True)
        sensor.skip_frames(50)
        #EXPOSURE_TIME_SCALE = 1.2
        #sensor 自动曝光控制
        print("Initial exposure == %d" % sensor.get_exposure_us())
        sensor.set_auto_gain(False)
        sensor.set_auto_whitebal(False)
        sensor.skip_frames(50)
        current_exposure_time_in_microseconds = sensor.get_exposure_us()
        print("Current Exposure == %d" % current_exposure_time_in_microseconds)
        sensor.set_auto_exposure(False, \
            exposure_us = int(current_exposure_time_in_microseconds * EXPOSURE_TIME_SCALE))
    #功能： 寻找最大的目标，计算方式为像素面积排序
    #输入： 链表objects，坐标X,坐标y,像素宽,像素高,mode=0是色块，1是人脸坐标
    #输出： 最大目标元组,
    def find_max_object(self,objects,mode=0):
        max_size=0
        if mode==0:#色块元组
            max_object=None
        if mode==1:#人脸框坐标格式和色块不同，单独定义
            max_object=(0,0,0,0,0.0)
        for object in objects:
            if object[2]*object[3] > max_size:
                max_object=object
                max_size = object[2]*object[3]
        return max_object
    #功能： 标定颜色阈值
    #输入：img,lcd,COLOR_THRESHOLD之前的颜色阈值,LAB_ERROR颜色阈值差,ROI_S窗口大小
    #输出： 标定后的颜色阈值
    def auto_color_mask(self,img,lcd,COLOR_THRESHOLD,LAB_ERROR,ROI_S):
        THRESHOLD=COLOR_THRESHOLD
        ROI=(int(img.width()/2-ROI_S/2),int(img.height()/2-ROI_S/2),ROI_S,ROI_S)
        if self.button.event():
            self.button.key_event=0
            while not self.button.event():
                img = sensor.snapshot()
                img.draw_rectangle(ROI)
                lcd.display(img)
            if self.button.event():
                self.button.key_event=0
                img = sensor.snapshot()
                statistics=img.get_statistics(roi=ROI)
                img.draw_rectangle(ROI)
                color_l=statistics.l_mode()
                color_a=statistics.a_mode()
                color_b=statistics.b_mode()
                THRESHOLD=[(color_l - LAB_ERROR,color_l + LAB_ERROR,color_a - LAB_ERROR,color_a + LAB_ERROR,color_b - LAB_ERROR,color_b + LAB_ERROR)]
            print(THRESHOLD)
        return THRESHOLD
    #假设距离变化每次控制在30左右，用来控制速度快慢
    #速度控制，主要用来控制速度变化快慢，达到匀速效果
    def speed_error_control(self,object_s,power,high_s,low_s):
        power_s=power
        s_error=self.s_last-object_s            #计算两次距离变化，前进时距离在接近
        self.s_last=object_s                    #更新s_last
        if s_error>= high_s :            #变化太快
            power_s=power_s-s_error*0.2  #对power进行修正
        if abs(s_error)<low_s:           #变化太慢或基本没有变化
            power_s=power_s+5            #对power进行修正
        return power_s

    def car_traking(self,object_s,pan_error,target_s,min_speed,max_speed,len_pid,dis_pid):
        #############################小车追踪############################
        power,power_s,power_l=0,0,0
        #前后PID
        if object_s>50 and object_s<=1000:
            dis_error=object_s-target_s #要产生后退的效果就把目标距离调大
            power_s=int(dis_pid.get_pid(dis_error,1)/2)
            if power_s>0:
                if power_s>max_speed:
                    power_s=max_speed
                if power_s<min_speed:
                    power_s=min_speed
                power_l=int(len_pid.get_pid(pan_error,1)/2)
            if power_s<0:
                if power_s<-max_speed:
                  power_s=-max_speed
                if power_s>-min_speed:
                  power_s=-min_speed
                  power_l=0
            print("power_s:",power_s)
            print("power_l:",power_l)
        return power_s,power_l
    ###################小车前后左右固定方向控制#####################
    def run_forward(self,speed,utime):
        self.motor.run(speed,speed)
        time.sleep(utime) #前进2s后停止
        self.motor.run(0,0)

    def run_back(self,speed,utime):
        self.motor.run(-speed,-speed)
        time.sleep(utime) #后退2s后停止
        self.motor.run(0,0)

    def run_left(self,speed,utime):    #左转后前进
        self.motor.run(speed,-speed)
        time.sleep(int(utime/4))  #转90度
        self.motor.run(60,60)
        time.sleep(utime)
        self.motor.run(0,0)

    def run_right(self,speed,utime):   #右转后前进
        self.motor.run(-speed,speed)
        time.sleep(int(utime/4))  #转90度
        self.motor.run(speed,speed)
        time.sleep(utime)
        self.motor.run(0,0)

    def run_round(self,speed,utime):   #转圈
        self.motor.run(-speed,speed)
        time.sleep(utime)
        self.motor.run(0,0)
